#include "modbus.h"
#include "serial.h"
#include "traceprint.h"
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <errno.h>


/*************************************************************/
/* Table of CRC values for high-order byte */
static unsigned char table_crc_hi[] = {
	0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 
	0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 
	0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 
	0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 
	0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 
	0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 
	0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 
	0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 
	0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 
	0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 
	0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 
	0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 
	0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 
	0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 
	0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 
	0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 
	0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 
	0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 
	0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 
	0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 
	0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 
	0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 
	0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 
	0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 
	0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 
	0x80, 0x41, 0x00, 0xC1, 0x81, 0x40
};

/* Table of CRC values for low-order byte */
static unsigned char table_crc_lo[] = {
	0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 
	0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 
	0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 
	0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 
	0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 
	0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3, 
	0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 
	0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 
	0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 
	0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 
	0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 
	0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, 
	0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 
	0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 
	0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 
	0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 
	0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 
	0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, 
	0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 
	0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 
	0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 
	0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 
	0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, 
	0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, 
	0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 
	0x43, 0x83, 0x41, 0x81, 0x80, 0x40
};

/* Fast CRC */
static unsigned short crc16(unsigned char *buffer,
			    unsigned short buffer_length)
{
	unsigned char crc_hi = 0xFF; /* high CRC byte initialized */
	unsigned char crc_lo = 0xFF; /* low CRC byte initialized */
	unsigned int i; /* will index into CRC lookup */

	/* pass through message buffer */
	while (buffer_length--) {
		i = crc_hi ^ *buffer++; /* calculate the CRC  */
		crc_hi = crc_lo ^ table_crc_hi[i];
		crc_lo = table_crc_lo[i];
	}

	return (crc_hi << 8 | crc_lo);
}

static int check_crc16( unsigned char *response, int response_size)
{
	int ret;
	
	unsigned short crc_calc;
	unsigned short crc_received;
	unsigned char recv_crc_hi;
	unsigned char recv_crc_lo;
	
	crc_calc = crc16(response, response_size - 2);
	
	recv_crc_hi = (unsigned) response[response_size - 2];
	recv_crc_lo = (unsigned) response[response_size - 1];
	
	crc_received = response[response_size - 2];
	crc_received = (unsigned) crc_received << 8;
	crc_received = crc_received | 
		(unsigned) response[response_size - 1];
	
	/* Check CRC of response */
	if (crc_calc == crc_received) 
	{
		ret = TRUE;
	}
	else
	{
		TRACE("ILLEGAL_CRC ");
		ret = ILLEGAL_CRC;
	}

	return ret;
}


/*********************************************************************/

static CSerial *openedPort = new CSerial;

void CModbusParam:: ModbusInitRTU( const char *deviceName,int iBaud, const char *cParity, int iDataBit,
	int iStopBit)
{
	
	baud = iBaud;
	bzero(parity, sizeof(parity));
	strncpy(parity, cParity, strlen(cParity));
	dataBit = iDataBit;
	stopBit = iStopBit;
	bzero(device, sizeof(device));
	strncpy(device, deviceName, strlen(deviceName));

	typeCom = RTU;
	headerLength =  HEADER_LENGTH_RTU;
	checksumSize = CHECKSUM_SIZE_RTU;
	
}

int CModbusParam::ModbusConnect()
{
	if (typeCom == RTU)
	{
		return _ModbusConnectRTU();
	}
	return FALSE;
}

int CModbusParam::_ModbusConnectRTU()
{
	int iParity = -1;
    	serial = openedPort;
	if(strncmp(parity,"even",4) == 0)
	{
		iParity = 1;
	}
	else if(strncmp(parity,"odd",3) == 0)
	{
		iParity = 2;
	}
	else if(strncmp(parity,"none",4) == 0)
	{
		iParity = 0;
	}
	
	if(serial->SerialOpen(device, baud, iParity, dataBit, stopBit) == FALSE)
	{
		fd = -1;
		return FALSE;
	}
	fd = serial->GetFd();
	
	return TRUE;
}

int CModbusParam::ModbusClose()
{
    if (serial != NULL)
    {
        delete serial;
    }
    return 0;
}

CModbusParam::~CModbusParam()
{
    ModbusClose();
}

/*****************************************************************************************************/

int CModbus::_BuildPackage( int slave,int function, int startAddr, int count, 
	unsigned char *package)
{	
	package[0] = slave;
	package[1] = function;
	package[2] = startAddr >> 8;
	package[3] = startAddr & 0x00FF;
	package[4] = count >> 8;
	package[5] = count & 0x00FF;

	return  PRESET_QUERY_SIZE_RTU;
}

int CModbus::_ComputeReturnSize(int function, int count)
{
	int ret = 0;
	switch(function)
	{
		case 0x01:
		case 0x02:
		{
			ret = 5+((count%8)?(count/8+1):(count/8));
			break;
		}
		case 0x03:
		case 0x04:
		{
			ret = 5+ count*2;
			break;
		}
		case 0x05:
		case 0x06:
		{
			ret = 8;
			break;
		}
		case 0x0f:
		case 0x10:
		{
			ret = 8;
			break;
		}
		default:
		{
			ret = FUNCTION_ERROR;
			break;
		}
	}
	TRACE("ret,function,count:%d,%d,%d",ret,function,count);
	return ret;
}

/**
return 
	success TRUE
	fail FALSE
**/
int CModbus::_SendQuery(CModbusParam *mbParam, unsigned char * query,int querySize)
{

	int ret = write(mbParam->fd,query,querySize);
	if (ret == -1 || ret != querySize)
	{
		TRACE("write failure");
		return WRITE_FAILURE;
	}
	return TRUE;
}


#define WAIT()	\
{\
	while ((selectRet = select(mbParam->fd+1, &rfds, NULL, NULL, &tv)) == -1) \
        {	\
	        if (errno == EINTR) {											\
		        FD_ZERO(&rfds);											\						
			FD_SET(mbParam->fd, &rfds);					\
		}\
	        else \
		{								\
			TRACE("select failure");		\
			return SELECT_FAILURE;						\
		}								\
	}																					\
}



	
/**
return 
	success TRUE
**/
int CModbus::_ReceiveData(CModbusParam *mbParam,unsigned char *response,int responseSize,
	unsigned char *query)
{

	fd_set rfds;
	struct timeval tv;
	FD_ZERO(&rfds);
	FD_SET(mbParam->fd, &rfds);
	tv.tv_sec = 0;
	tv.tv_usec = TIME_OUT_BEGIN_OF_TRAME;
	int selectRet = 0;
	int ret;
	int readSize = 0;

	WAIT();
	 if (selectRet == 0)
        {								
                /* Call to error_treat is done later to manage exceptions */	
                TRACE("comm_time_out");
		return COMM_TIME_OUT;					
	}
	ret = read(mbParam->fd, response, 1);
	if(ret != 1)
	{
		TRACE("read_error");
		return  READ_FAILURE;
	}
	if (response[0] != query[0])
	{
		TRACE("slave is wrong");
		return  ILLEGAL_SLAVE;
	}
	readSize = ret;
		
	while(1)
	{
		ret = read(mbParam->fd, response+readSize,responseSize-readSize);
		if(ret < 0)
		{	
			TRACE("read_error");
			return READ_FAILURE;
		}

		TRACE("ret = %d",ret);
		readSize +=ret;
		if(readSize < responseSize)
		{
			WAIT();
			 if (selectRet == 0)
		        {								
		                /* Call to error_treat is done later to manage exceptions */	
		                //TRACE("comm_time_out");
				//return COMM_TIME_OUT;
				TRACE("have read %d data",readSize);
		                break;
			}
		}
		//else if(readSize > responseSize)
		//{
		//	return TOO_MANY_DATAS ;
		//}
		else
		{
			break;
		}
	}

	TRACE("readSize=%d",readSize);

	WAIT();
	if(selectRet > 0)
	{
		TRACE("too_many_data");
		return TOO_MANY_DATAS ;
	}

	if(readSize < 2)
	{
		TRACE("the length of response is less than 2");
		return TOO_LESS_DATAS;
	}

	if (response[1] == (query[1]|0x80))
	{
		if (readSize != ERROR_RESPONSE)
		{
			TRACE("the length is too less");
			return TOO_LESS_DATAS;
		}
		else
		{
			TRACE("success return the exception");
			return EXCEPTION;
		}
	}
	else if (response[1] == query[1])
	{
		if (readSize != responseSize)
		{
			TRACE("the length is too less");
			return TOO_LESS_DATAS;
		}
	}
	else
	{
		TRACE("function error");
		return FUNCTION_ERROR;
	}
    	if (( ret= check_crc16(response, readSize)) != TRUE)
	{
		return ret;
	}

    	return TRUE;
        
}

int CModbus::_SendAndReceive(CModbusParam *mbParam, int function,int slave,
		     int startAddr, int count, unsigned char  *response)
{
	unsigned char query[MIN_QUERY_SIZE] = {0};
	int querySize = 0,responseSize = 0;
	querySize = _BuildPackage(slave, function, startAddr, count, query);

	unsigned short s_crc;
	s_crc = crc16(query, querySize);
	query[querySize++] = s_crc >> 8;
	query[querySize++] = s_crc & 0x00FF;

	int ret = 0;
	ret = _SendQuery(mbParam, query, querySize);
	if(ret !=TRUE)
	{
		TRACE("send package failure:%s",query);
		return ret;
	}
	
	responseSize = _ComputeReturnSize(function, count);
	if (responseSize < 0)
	{
		return responseSize;
	}
	
	ret = _ReceiveData(mbParam,  response, responseSize, query);
	return ret;
}

int CModbus::_ReadBitIO(CModbusParam *mbParam, int function,int slave,
		     int startAddr, int count, int *dest)
{
	if (slave < 0 || slave > MAX_SLAVE_ADDR)
	{
		return ILLEGAL_SLAVE;
	}
	if (count < 0 || count > MAX_READ_STATUS)
	{
		return ILLEGAL_DATA_COUNT;
	}	
	if (startAddr < 0 || startAddr+count > MAX_START_ADDRESS)
	{
		return ILLEGAL_DATA_ADDRESS;
	}
	if (dest == NULL || mbParam == NULL)
	{
		return POINT_IS_NULL;
	}
		
	unsigned char  response[MAX_PACKET_SIZE] = {0};
	int ret = _SendAndReceive( mbParam, function, slave, startAddr, count, response);
	if(ret == TRUE)
	{	
		unsigned char temp;
		int c = response[2];//size of data
		for(int i = 0, k = 0; i < c; i++)
		{
			temp = response[3+i];
			for(int j = 0;j < 8&& k < count; j++, k++)
			{
              		        dest[k] = (temp & 0x01)?0x0001:0x00;
				TRACE("dest[%d] = %x",k,dest[k]);
				temp = temp >> 1;
			}
		}
	}

	return ret;
}

/**
return 
	success TRUE
	fail FALSE
**/
int CModbus::ReadCoilStatus(CModbusParam *mbParam, int slave,
	     int startAddr, int count, int *dest)
{
	int function = 0x01;
	return _ReadBitIO(mbParam, function, slave, startAddr, count ,dest);
}

int CModbus::ReadInputStatus(CModbusParam *mbParam, int slave,
	      int startAddr, int count, int *dest)
{
	int function = 0x02;
	return _ReadBitIO(mbParam, function, slave, startAddr, count ,dest);
}

int CModbus::_ReadRegIO(CModbusParam *mbParam, int function,int slave,
		     int startAddr, int count, int *dest)
{
	if (slave < 0 || slave > MAX_SLAVE_ADDR)
	{
		return ILLEGAL_SLAVE;
	}
	if (count < 0 || count > MAX_READ_REGS)
	{
		return ILLEGAL_DATA_COUNT;
	}	
	if (startAddr < 0 || startAddr+count > MAX_START_ADDRESS)
	{
		return ILLEGAL_DATA_ADDRESS;
	}
	if (mbParam == NULL || dest == NULL)
	{
		return POINT_IS_NULL;
	}

	unsigned char  response[MAX_PACKET_SIZE] = {0};
	int ret = _SendAndReceive( mbParam, function, slave, startAddr, count, response);
	if(ret == TRUE)
	{	
        //unsigned char temp;
		for(int k = 0,i = 0; k < count; k++,i += 2)
		{
			dest[k] = response[3+i];
			dest[k] = (dest[k]<<8) + response[3+i+1];
			TRACE("dest[%d] = %x,response=%x,%x",k,dest[k],response[3+i],response[3+1+i]);
		}
	}
	
	return ret;
}

int CModbus::ReadHoldingRegisters(CModbusParam *mbParam, int slave,
		   int startAddr, int count, int *dest)
{
	int function = 0x03;
	return _ReadRegIO(mbParam, function, slave, startAddr, count ,dest);
}

int CModbus::ReadInputRegisters(CModbusParam *mbParam, int slave,
		 int startAddr, int count, int *dest)
{
	int function = 0x04;
	return _ReadRegIO(mbParam, function, slave, startAddr, count ,dest);
}


int CModbus::_WriteSingle(CModbusParam *mbParam, int slave,int function,
	      int startAddr, int value)
{
	if (slave < 0 || slave > MAX_SLAVE_ADDR)
	{
		return ILLEGAL_SLAVE;
	}	
	if (startAddr < 0 || startAddr > MAX_START_ADDRESS)
	{
		return ILLEGAL_DATA_ADDRESS;
	}
	if (mbParam == NULL)
	{
		return POINT_IS_NULL;
	}
	
	unsigned char  response[MAX_PACKET_SIZE] = {0};
	return _SendAndReceive( mbParam, function, slave, startAddr, value, response);
}

int CModbus::ForceSingleCoil(CModbusParam *mbParam, int slave,
	      int coilAddr, int state)
{
	int function = 0x05;
	if (state != 0)
	{
		state = 0xff00;
	}
	else 
	{
		state = 0x0;
	}
	return _WriteSingle(mbParam, slave,function, coilAddr, state);
}

int CModbus::PresetSingleRegister(CModbusParam *mbParam, int slave,
		   int regAddr, int value)
{
	int function = 0x06;
	return _WriteSingle(mbParam, slave, function, regAddr, value);
}


int CModbus::_WriteMultiple(CModbusParam *mbParam, int slave,int function, 
		 int startAddr, int count,int byteSize,  unsigned char *data)
{
	unsigned char  response[MAX_PACKET_SIZE] = {0};
	unsigned char query[MAX_PACKET_SIZE] = {0};
	int querySize = 0,responseSize = 0;
	querySize = _BuildPackage(slave, function, startAddr, count, query);
	query[querySize++] = byteSize;
	for(int i = 0; i < byteSize; i++)
	{
		query[querySize++] = data[i];
	}


	unsigned short s_crc;
	s_crc = crc16(query, querySize);
	query[querySize++] = s_crc >> 8;
	query[querySize++] = s_crc & 0x00FF;

	int ret = 0;
	ret = _SendQuery(mbParam, query, querySize);
	if(ret ==FALSE)
	{
		TRACE("send package failure:%s",query);
		ret = WRITE_FAILURE;
		return ret;
	}
	
	responseSize = _ComputeReturnSize(function, count);
	if (responseSize < 0)
	{
		return responseSize;
	}
	ret = _ReceiveData(mbParam,  response, responseSize, query);
    return ret;
}

int CModbus::ForceMultipleCoils(CModbusParam *mbParam, int slave,
		 int startAddr, int coil_count, int *data)
{

	if (slave < 0 || slave > MAX_SLAVE_ADDR)
	{
		return ILLEGAL_SLAVE;
	}
	if (coil_count < 0 || coil_count> MAX_WRITE_STATUS)
	{
		return ILLEGAL_DATA_COUNT;
	}	
	if (startAddr < 0 || startAddr+coil_count> MAX_START_ADDRESS)
	{
		return ILLEGAL_DATA_ADDRESS;
	}
	if (data == NULL || mbParam == NULL)
	{
		return POINT_IS_NULL;
	}
	
	int function = 0x0f;
	int byteSize = ((coil_count%8)?(coil_count/8+1):(coil_count/8));
	//test
	
	unsigned char bitData[(MAX_WRITE_STATUS%8)?(MAX_WRITE_STATUS/8+1):(MAX_WRITE_STATUS/8)] = {0};
	TRACE("bitDataSize=%d",sizeof(bitData));
	for(int i = 0, k = 0; i < byteSize; i++)
	{
		for(int j = 0; j < 8; j++,k++)
		{
			if(data[k] == 0 || k >= coil_count)
			{
				bitData[i] = bitData[i] & 0x7f;
			}
			else 
			{
				bitData[i] = bitData[i]  | 0x80;
			}
			
			if (j != 7 )
			{
				TRACE("k =%d",k);
				bitData[i] = (bitData[i] >> 1);
			}
		}
		TRACE("bitData=%x",bitData[i]);
	}
	return _WriteMultiple(mbParam, slave, function,startAddr, coil_count, byteSize, bitData);
	
}

int CModbus::PresetMultipleRegisters(CModbusParam *mbParam, int slave,
		      int startAddr, int regCount, int *data)
{
	if (slave < 0 || slave > MAX_SLAVE_ADDR)
	{
		return ILLEGAL_SLAVE;
	}
	if (regCount < 0 || regCount> MAX_WRITE_REGS)
	{
		return ILLEGAL_DATA_COUNT;
	}	
	if (startAddr < 0 || startAddr+regCount> MAX_START_ADDRESS)
	{
		return ILLEGAL_DATA_ADDRESS;
	}
	if (data == NULL || mbParam == NULL)
	{
		return POINT_IS_NULL;
	}

	int function = 0x10;
	int byteSize = regCount * 2;
	//test
	//unsigned char regData[byteSize] = {0};
	unsigned char regData[MAX_WRITE_REGS*2] = {0};
	for(int i = 0, k = 0; i < regCount; i++, k+=2)
	{
		regData[k] = data[i] >> 8;
		regData[k+1] = data[i] & 0x00ff;
		TRACE("i = %d,k=%x,k+1=%x",i,regData[k],regData[k+1]);
	}
	return  _WriteMultiple(mbParam, slave, function, startAddr, regCount, byteSize, regData);

}

/******************************************************/
